#include "../../MyUtils/Graph/Graph.hpp"
#include <iostream>
#include <functional>

using namespace std;

class Coordinate
{
  private:
    int __x, __y; //坐标

  public:
    Coordinate( int x = -1, int y = -1 ) : __x( x ), __y( y ) {}
    inline int x( ) const { return __x; }
    inline int y( ) const { return __y; }
    inline bool operator==( const Coordinate &rhs ) const
    {
        return x( ) == rhs.x( ) && y( ) == rhs.y( );
    }
    inline bool operator!=( const Coordinate &rhs ) const
    {
        return !( *this == rhs );
    }
    inline bool operator<( const Coordinate &rhs ) const
    {
        return ( __x < rhs.__x ) || ( __x == rhs.__x && __y < rhs.__y );
    }
    inline bool operator>( const Coordinate &rhs ) const
    {
        return !( *this == rhs ) && !( *this < rhs );
    }
};

class MyGraph : public Graph::Algorithm::GraphInterface
{
  public:
    vector< vector< int > > __graph;
    Coordinate __start, __goal;

  public:
    MyGraph( const vector< vector< int > > &graph, const Coordinate &start,
             const Coordinate &goal )
        : __graph( graph ), __start( start ), __goal( goal )
    {
    }

  private:
  public:
};

double g_cost( const Coordinate &start, const Coordinate &current )
{
    return abs( start.x( ) - current.x( ) ) + abs( start.y( ) - current.y( ) );
}

double h_cost( const Coordinate &goal, const Coordinate &current )
{
    return abs( goal.x( ) - current.x( ) ) + abs( goal.y( ) - current.y( ) );
}

inline bool is_coordinate_valid( const MyGraph &graph, const Coordinate &n )
{
    if ( n.x( ) >= 0 && n.x( ) < 10 )
    {
        if ( n.y( ) >= 0 && n.y( ) < 10 )
        {
            if ( graph.__graph[ n.x( ) ][ n.y( ) ] == 0 )
                return true;
        }
    }
    return false;
}

std::vector< Coordinate >
neighbors( const Graph::Algorithm::GraphInterface &graph,
           const Coordinate &current )
{
    std::vector< Coordinate > tmp = {
        Coordinate( current.x( ), current.y( ) + 1 ),
        Coordinate( current.x( ), current.y( ) - 1 ),
        Coordinate( current.x( ) + 1, current.y( ) ),
        Coordinate( current.x( ) - 1, current.y( ) ),
    };
    std::vector< Coordinate > ret;

    for ( auto iter = tmp.begin( ); iter != tmp.end( ); ++iter )
    {
        if ( is_coordinate_valid( *( ( MyGraph * ) &graph ), *iter ) )
        {
            ret.push_back( *iter );
        }
    }
    return ret;
}

void printPath( vector< Coordinate > path )
{

    for ( auto it = path.rbegin( ); it != path.rend( ); ++it )
    {
        printf( "(%d, %d)\n", it->x( ), it->y( ) );
    }
}

int main( )
{
    Coordinate start( 4, 0 );
    Coordinate goal( 0, 6 );
    vector< vector< int > > g = {
        { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
        { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
        { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
        { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
        { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
    }; //纵轴是x，横轴是y，左上角是原点
    MyGraph graph( g, start, goal );

    std::function<double(const Coordinate&)> f_cost = [ &start, &goal ]( const Coordinate &current )->double {
        return g_cost( start, current ) + h_cost( goal, current );
    };

    vector< Coordinate > path =
        Graph::Algorithm::Search< Coordinate >::a_star_search(
            *( ( Graph::Algorithm::GraphInterface * ) &graph ), start, goal,
            neighbors, f_cost );

    printf( "A*算法输出:\n" );
    printPath( path );

    return 0;
}
